/*=============================================================================
#    The element-comm is a tentative program used to test our ideas.           
#                       Copyright (C)  bgao                                    
#
#     This program is free software; you can redistribute it and/or            
#      modify it under the terms of the GNU General Public License             
#     as published by the Free Software Foundation; either version 2           
#         of the License, or (at your option) any later version.               
#
#     This program is distributed in the hope that it will be useful,          
#    but WITHOUT ANY WARRANTY; without even the implied warranty of           
#     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the            
#             GNU General Public License for more details.                    
#
#    You should have received a copy of the GNU General Public License        
#     along with this program; if not, write to the Free Software             
#      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,             
#                       MA  02110-1301, USA.                                  
#==============================================================================
# $Id$ 
#
# $Author$ 
#
# $Revision$ 
#
# $Date$ 
#
# Description: 
#
=============================================================================*/
#define WSN_GLOBALS
#include    "rtos-wsn.h"

static uint8 wsn_ready_tasks;
static uint8 wsn_sorted_tasks;
static uint8 wsn_want_sched;

extern struct wsn_task_attr wsn_task_attr_list[] FLASH;
static struct wsn_task_resource wsn_resource_table[WSN_MAX_RESOURCE_NUM] =
						{{1, 2000}, {2, 0}, {3, 0}, {4, 0}, {5, 0}
						};


#define TaskAttr(taskid)	((struct wsn_task_attr *)(&wsn_task_attr_list[taskid]))

void wsn_task_init(void)
{
    struct wsn_empty_task * p_task;
    int i;
	
    /* Disable sched */
    wsn_disable_sched = SCHED_LOCK;
    /* Initialize empty task list */
    p_task = (struct wsn_empty_task *)&wsn_task_list[0];
    wsn_empty_task_list = p_task;
    for ( i = 0; i < WSN_MAX_ACTIVE_TASKS - 1; i ++ ) {
        p_task->u.task.state = TASK_COMPLETE;
		p_task->u.next = (struct wsn_empty_task *)&wsn_task_list[i+1];
		p_task = (struct wsn_empty_task *)&wsn_task_list[i+1];
    }
    p_task->u.next = 0;
}

static uint8 Wsn_SortTask (void)
{
    struct wsn_task * p_parent, * p_child;
    uint8 parent, child, retval = 0;
	
    while ( wsn_sorted_tasks < wsn_ready_tasks ) {
		child = ++wsn_sorted_tasks;
		p_child = wsn_task_queue[child];
		while ( (parent = child / 2) > 0 ) {
			p_parent = wsn_task_queue[parent];
			if ( (long)(p_child->deadline - p_parent->deadline) >= 0 )
				break;
			/* Switch parent and child */
			wsn_task_queue[child] = p_parent;
			child = parent;
		}
        wsn_task_queue[child] = p_child;
		if ( child <= 1 )
			retval = 1;
    }
    return retval;
}

static void Wsn_TaskShiftDown (struct wsn_task * p_task, uint8 index)
{
    struct wsn_task * p_child;
    uint8 child;
	
    while ( (child = index * 2) <= wsn_sorted_tasks ) {
		p_child = wsn_task_queue[child];
		if ( child < wsn_sorted_tasks ) { /* This node has 2 children */
			struct wsn_task * p_rchild = wsn_task_queue[child+1];
			if ( (long)(p_rchild->deadline - p_child->deadline) < 0 ) {
				p_child = p_rchild;
				child ++;
			}
		}
		if ( (long)(p_task->deadline - p_child->deadline) <= 0 )
			break;
		/* Swap parent and child */
		wsn_task_queue[index] = p_child;
		index = child;
    }
    wsn_task_queue[index] = p_task;
}

/********************************************************************
*
* When call this function, the wsn_sched_mode must be TASK_SCHED.
*
* wsn_sched_enable must not 0.
*
********************************************************************
*/

static void Wsn_TaskSched (void)
{
    struct wsn_task * p_current, * p_head;
	struct wsn_task_section * p_sec;
	struct wsn_task_attr * p_attr;
	
    int (*task_entry)(int);
    int event, retval;
	
    if ( wsn_task_nopreempt )
		return;

    p_current = wsn_task_queue[0];
    /* Now, task sched is disabled */
    while ( wsn_ready_tasks != 0 ) {
        /* Sort the task heap */
		if ( wsn_sorted_tasks < wsn_ready_tasks ) {
			Wsn_SortTask();
		}

		p_head = wsn_task_queue[1];
	/* EDF Schedule*/
	//if ( p_current && (long)(p_head->deadline - p_current->deadline) >= 0 )
		//	break;
	/*EDFI Schedule*/
	p_attr = TaskAttr(p_head->taskindex);
	if ( p_current && 
		   ( (long)(p_head->deadline - p_current->deadline) >= 0 || flash_read_dword(p_attr->time_to_complete) >= p_current->delta) )
		 break;

		/* Select head task, remove head task from heap */
		wsn_EnterCritical();
		if ( wsn_ready_tasks > 1 ) {
			struct wsn_task * p_task = wsn_task_queue[wsn_ready_tasks --];
			if ( wsn_sorted_tasks > wsn_ready_tasks )
				wsn_sorted_tasks = wsn_ready_tasks;
			wsn_LeaveCritical();
			Wsn_TaskShiftDown(p_task, 1);
		} else {
			wsn_sorted_tasks = wsn_ready_tasks = 0;
			wsn_LeaveCritical();
		}

		wsn_task_queue[0] = p_head;
		do {
			/*get the entry from flash*/
			p_attr = TaskAttr(p_head->taskindex);

			p_sec = (struct wsn_task_section *)flash_read_word(p_attr->section_list);
			task_entry = (fn_int_ptr)flash_read_word(p_sec[p_head->section].entry);

			wsn_EnterCritical();
			event = p_head->event;
			p_head->event = 0;
			wsn_LeaveCritical();
			//WSN_SchedOn();//recursive sched 
			/* Call task section entry */
			retval = task_entry(event);
			/* Clear no preempt flag */
			wsn_task_nopreempt = 0;
			/* Disable task sched again */
			//WSN_SchedOff();
		} while ( retval == 0 && p_head->event != 0 );
	    
		p_head->state = TASK_BLOCKED;
		if ( retval != 0 ) { /* Free this task into free list */
			struct wsn_empty_task * p_task = (struct wsn_empty_task *)p_head;
			p_head->state = TASK_COMPLETE;
			wsn_EnterCritical();
	        p_task->u.next = wsn_empty_task_list;
			wsn_empty_task_list = p_task;
			wsn_LeaveCritical();
		}
    }

    wsn_task_queue[0] = p_current;
}

#ifdef _Debug_thread_

static void Wsn_ThreadSched(void)
{
    struct wsn_thread * p_current, * pt, ** pp;
	
    p_current = wsn_current_thread;
    /* compare priority to thread switch */
    if ( (pt = wsn_thread_queue) != 0 && pt->priority > p_current->priority ) {
		wsn_current_thread = pt;
		wsn_thread_queue = pt->next;
		pp = &wsn_thread_queue;
		while ( (pt = *pp) != 0 && pt->priority > p_current->priority )
			pp = &pt->next;
		*pp = p_current;
		p_current->next = pt;
		wsn_EnterCritical();
		wsn_context_switch(&p_current->context, &wsn_current_thread->context);
		wsn_LeaveCritical();
    }
}
#endif

void Wsn_Backgroundtask_Sched(void)
{
		for (;;)
			wsn_run_next_backtask();
}


void WSN_SchedOn(void)
{
	if(wsn_int_nesting){
		wsn_disable_sched = SCHED_UNLOCK;
		return;
	}
	while ( wsn_want_sched ) {
		wsn_want_sched = WANTSCHED_RESET;
		if ( wsn_sched_mode == TASK_SCHED ) {
			Wsn_TaskSched();
		} 
#ifdef _Debug_thread_

		else if ( wsn_sched_mode == THREAD_SCHED ) {
	    if ( wsn_ready_tasks != 0 ) { /* Switch to thread 0 */
				wsn_sched_mode = TASK_SCHED;
				wsn_EnterCritical();
				wsn_context_switch(&wsn_current_thread->context, &wsn_thread0_context);
				wsn_LeaveCritical();
			} else {
				Wsn_ThreadSched();
			}
		}
#endif
    }
    wsn_disable_sched = SCHED_UNLOCK;
}

int16 WSN_NewTask(uint16 taskid, uint16 event, int16 time)
{
    struct wsn_empty_task * p_empty_task;
    struct wsn_task_attr * p_task_attr;
	struct wsn_task_section * p_task_sec;
    struct wsn_task * p_task;
	
    p_task_attr = TaskAttr(taskid);
	
    /* Find an empty task structure */
    wsn_EnterCritical();
    p_empty_task = wsn_empty_task_list;

	if((uint32)p_empty_task > (uint32)&wsn_task_list[WSN_MAX_ACTIVE_TASKS]){
		while(1);
	}
    ASSERT (p_empty_task != 0);
	if ((p_empty_task != 0)== 0){
		//return ERR_NO_EMPTY_TASK;
		while(1);
	}
    wsn_empty_task_list = p_empty_task->u.next;
	/*check the wsn_empty_task_list*/
	if((uint32)wsn_empty_task_list->u.next > (uint32)&wsn_task_list[WSN_MAX_ACTIVE_TASKS]){
		while(1);
	}
    wsn_LeaveCritical();
    p_task = (struct wsn_task *)p_empty_task;

	/*check if the space has been inited*/
	if(p_task->state != TASK_COMPLETE){
		while(1);
	}
	
    p_task->taskindex = taskid;
    p_task->state = TASK_READY;
    p_task->section = 0;
    p_task->event = event;	
	p_task->delta = 0xffffffff;/*for EDFI,should be a infinite value*/
	
    if ( time == 0 ) {
        //p_task->deadline = Wsn_TaskSchedTime() + p_task_attr->time_to_complete;
        
                p_task->deadline = Wsn_TaskSchedTime() + flash_read_dword(p_task_attr->time_to_complete);
		if ( flash_read_byte(p_task_attr->n_sections) > 1 ){
			p_task_sec = (struct wsn_task_section *) flash_read_word(p_task_attr->section_list);
			p_task->deadline -= flash_read_word(p_task_sec[0].advance);
		}
	} else {
		struct wsn_task * p_current = wsn_task_queue[0];
		if(p_current == NULL)
			p_task->deadline =  time;
		else
			p_task->deadline = p_current->deadline + time;
    }
    /* Add to end of queue */
    wsn_EnterCritical();
    wsn_task_queue[++wsn_ready_tasks] = p_task;
    wsn_LeaveCritical();
    wsn_want_sched = WANTSCHED_SET;

    if ( ! wsn_disable_sched )
		WSN_SchedOn();

    return (p_task - wsn_task_list) + 1;
}

int16 WSN_RestartTask (uint8 taskhandle, uint16 taskid, uint8 section_id, uint16 event)
{
    struct wsn_task * p_task;
    struct wsn_task_attr * p_task_attr;
	struct wsn_task_section * p_task_sec;
	
    ASSERT (taskhandle > 0 && taskhandle <= WSN_MAX_ACTIVE_TASKS);
	if((taskhandle > 0 && taskhandle <= WSN_MAX_ACTIVE_TASKS)==0)
			return ERR_TASKHANDLE_INVALID;
    p_task = &wsn_task_list[taskhandle - 1];
    ASSERT (p_task->taskindex == taskid);
	if((p_task->taskindex == taskid)==0)
		return ERR_TASKID_NOT_MATCH;
    p_task_attr = TaskAttr(taskid);

	p_task_sec = (struct wsn_task_section *) flash_read_word(p_task_attr->section_list);
	
    if ( p_task->section != section_id ) {
		int new_advance;
		new_advance = flash_read_word(p_task_sec[p_task->section].advance) -
			flash_read_word(p_task_sec[section_id].advance);
		
        p_task->section = section_id;
		p_task->deadline += new_advance;
    }
	
    wsn_EnterCritical();
    p_task->event |= event;
    if ( p_task->state == TASK_BLOCKED ) {
		p_task->state = TASK_READY;
        wsn_task_queue[++wsn_ready_tasks] = p_task;
    }
    wsn_LeaveCritical();
    wsn_want_sched = WANTSCHED_SET;
    if ( ! wsn_disable_sched )
		WSN_SchedOn();
    return 0;
}

uint8 WSN_TaskHandle(uint16 taskid)
{
    uint8 i;
	
    if ( taskid == 0 )
		return (wsn_task_queue[0] - wsn_task_list) + 1;
    /* Find task with this ID */
    for ( i = 0; i < WSN_MAX_ACTIVE_TASKS; i ++ ) {
		if ( wsn_task_list[i].taskindex == taskid && wsn_task_list[i].state != TASK_COMPLETE )
			return i + 1;
    }
    return 0;
}


int16 WSN_TaskID(uint8 taskHandle)
{
    struct wsn_task * p_task;
	
	if ( taskHandle == 0 || taskHandle > WSN_MAX_ACTIVE_TASKS )
		return ERR_TASKHANDLE_INVALID;

    p_task = &wsn_task_list[taskHandle - 1];

    if ( p_task->taskindex < 0 || p_task->state == TASK_COMPLETE )
		return -1;
	
    return p_task->taskindex;
}
#ifdef _Debug_thread_

static int wsn_AddReadyQueue(struct wsn_thread * thread)
{
    struct wsn_thread * prev, * pt;
    uint8 pos = 0;
	
    thread->state = THREAD_READY;
    if ((prev = wsn_thread_queue) == 0 || prev->priority < thread->priority) {
		thread->next = prev;
		wsn_thread_queue = thread;
    } else {
		pos ++;
		while ((pt = prev->next) != 0 && pt->priority >= thread->priority) {
			pos ++;
			prev = pt;
		}
		thread->next = pt;
		prev->next = thread;
    }
    return pos;
}

struct block_timer_s {
    struct wsn_timer timer;
    struct wsn_thread * thread;
    struct wsn_thread ** waitq;
};

static void Wsn_ThreadTimeout(struct wsn_timer * timer)
{
    struct block_timer_s * p_blkt;
    struct wsn_thread * pt, ** pq;
    struct wsn_thread * thread;
	
    p_blkt = (struct block_timer_s *)timer;
    thread = p_blkt->thread;
    if ( thread->state == THREAD_BLOCKED ) {
		thread->error = -ETIMEDOUT;
	if ( (pq = p_blkt->waitq) != 0 ) { /* Take it out from wait queue */
			while ( (pt = *pq) != 0 ) {
				if ( pt == thread ) {
					*pq = thread->next;
					break;
				}
			}
		}

	
		wsn_AddReadyQueue(thread);
    }
}

static int Wsn_ThreadBlock(struct wsn_thread ** waitq, int timeout)
{
    struct wsn_thread * thread;
    struct block_timer_s block_timer;
	
    if ( timeout == 0 )
		return -ETIMEDOUT;
	
    thread = wsn_current_thread;
    thread->state = THREAD_BLOCKED;
    thread->error = 0;
    if ( waitq != 0 ) { /* Add to wait queue */
		struct wsn_thread * pt, ** pq;
		pq = waitq;
		while ( (pt = *pq) != 0 && pt->priority >= thread->priority )
			pq = &pt->next;
		*pq = thread;
		thread->next = pt;
    }

    if ( timeout > 0 ) {
		block_timer.thread = thread;
		block_timer.waitq = waitq;
        WSN_InitTimer(&block_timer.timer, Wsn_ThreadTimeout);
        WSN_StartTimer(&block_timer.timer, timeout);
    }

    if ( (wsn_current_thread = wsn_thread_queue) == 0 ) {
		wsn_sched_mode = 0;
		wsn_EnterCritical();
		wsn_context_switch(&thread->context, &wsn_thread0_context);
		wsn_LeaveCritical();
    } else {
		wsn_thread_queue = wsn_current_thread->next;
		wsn_EnterCritical();
		wsn_context_switch(&thread->context, &wsn_current_thread->context);
		wsn_LeaveCritical();
    }

    if ( timeout > 0 ) {
        WSN_StopTimer(&block_timer.timer);
    }

    return thread->error;
}

void WSN_Sleep(int timeout)
{
    if ( wsn_sched_mode != THREAD_SCHED )
		return;
    WSN_SchedOff();
    Wsn_ThreadBlock(0, timeout);
    WSN_SchedOn();
}

void WSN_SetPriority(uint8 priority)
{
    if ( wsn_sched_mode != THREAD_SCHED )
		return;
    if ( !(priority > 0 && priority < 32) )
		return;
    WSN_SchedOff();
    wsn_current_thread->priority = priority;
    wsn_want_sched = WANTSCHED_SET;
    WSN_SchedOn();
}

void WSN_InitEvent(struct wsn_event * p_event, uint8 auto_reset, uint8 state)
{
    p_event->wait = 0;
    p_event->auto_reset = auto_reset;
    p_event->state = state;
}

int WSN_WaitEvent(struct wsn_event * p_event, int timeout)
{
    int error;
	
    if ( wsn_sched_mode != THREAD_SCHED )
		return 0;
	
    WSN_SchedOff();
    if ( p_event->state != 0 ) {
		if ( p_event->auto_reset )
			p_event->state = 0;
   	    
		WSN_SchedOn();
		return 0;
    }

    error = Wsn_ThreadBlock(&p_event->wait, timeout);
    WSN_SchedOn();
    return error;
}

void WSN_SetEvent(struct wsn_event * p_event)
{
    struct wsn_thread * thread;
	
    if ( p_event->state != 0 )
		return;
    WSN_SchedOff();
    p_event->state = 1;
    if ( (thread = p_event->wait) != 0 ) {
		if ( p_event->auto_reset ) {
			p_event->state = 0;
			p_event->wait = thread->next;
			wsn_AddReadyQueue (thread);
		} else { /*Wakeup all threads*/ 
			do {
				p_event->wait = thread->next;
				wsn_AddReadyQueue (thread);
			} while ((thread = p_event->wait) != 0);
		}
    }
    wsn_want_sched = WANTSCHED_SET;
    WSN_SchedOn();
}

void WSN_ClearEvent(struct wsn_event * p_event)
{
    p_event->state = 0;
}

void WSN_InitSem(struct wsn_semaphore * p_sem, uint8 value)
{
    p_sem->wait = 0;
    p_sem->value = value;
}

int WSN_WaitSem(struct wsn_semaphore * p_sem, int timeout)
{
    int error = 0;
	
    if ( wsn_sched_mode != THREAD_SCHED )
		return 0;
	
    WSN_SchedOff();
    while ( ! error ) {
		if ( p_sem->value == 0 ) {
			error = Wsn_ThreadBlock(&p_sem->wait, timeout);
		} else {
			p_sem->value --;
			break;
		}
    }
    WSN_SchedOn();
    return error;
}

void WSN_SignalSem(struct wsn_semaphore * p_sem)
{
    struct wsn_thread * thread;
	
    WSN_SchedOff();
    p_sem->value ++;
    if ( (thread = p_sem->wait) != 0 ) {
        p_sem->wait = thread->next;
		wsn_AddReadyQueue (thread);
    }
    wsn_want_sched = WANTSCHED_SET;
    WSN_SchedOn();
}

extern struct wsn_thread_attr wsn_thread_attr_list[];

/* This is the start entry for all thread */
static void wsn_thread_entry(void * param, struct context_info * p_context)
{
    struct wsn_thread * p_thread;
    wsn_thread_entry_t thread_entry;
	
    p_thread = (struct wsn_thread *)((char *)p_context - (unsigned int)&(((struct wsn_thread *)0)->context));
	
    thread_entry = *(wsn_thread_entry_t *)(p_thread + 1);
    WSN_SchedOn();
	wsn_LeaveCritical();
    thread_entry();
    WSN_SchedOff();
    Wsn_ThreadBlock(0, -1);
}

void wsn_thread_init(void)
{
    struct wsn_thread_attr * p_attr = &wsn_thread_attr_list[0];
    struct wsn_thread * p_thread;
	
    for ( p_attr = &wsn_thread_attr_list[0]; p_attr->entry != 0; p_attr ++ ) {
		p_thread = (struct wsn_thread *)p_attr->p_stack;
		*(wsn_thread_entry_t *)(p_thread + 1) = p_attr->entry;
		p_thread->priority = p_attr->priority;
		p_thread->error = 0;
        /* Make context in stack */    
		wsn_context_init (&p_thread->context, /* Address of context */
		(char *)p_thread + p_attr->stacksize, /* Top of stack */
			(void (*)(void *))wsn_thread_entry, /* Entry */
			p_thread );/* Param of entry */
		wsn_AddReadyQueue(p_thread);
    }
}
#endif
void wsn_sched(void)
{
    for ( ; ; ) {
        wsn_disable_sched = SCHED_LOCK;
		wsn_sched_mode = TASK_SCHED;
		if ( wsn_ready_tasks != 0 ) {				
			Wsn_TaskSched();
		} 
#ifdef _Debug_thread_

		else if ( wsn_current_thread != 0 ) {		
			wsn_sched_mode = THREAD_SCHED;
			wsn_EnterCritical();			
			wsn_context_switch(&wsn_thread0_context, &wsn_current_thread->context);
			wsn_LeaveCritical();			
		} else if ( wsn_thread_queue != 0 ) {
			wsn_current_thread = wsn_thread_queue;
			wsn_thread_queue = wsn_current_thread->next;
		} 

#endif
		else {
			wsn_sched_mode = BACKGROUND_SCHED;
			wsn_disable_sched = SCHED_UNLOCK;
			Wsn_Backgroundtask_Sched();
			
		}
    }
}



void wsn_int_sched(void)
{

	if(wsn_disable_sched)
		return;
    if ( wsn_want_sched ) {
		wsn_want_sched = WANTSCHED_RESET;	
	
		if ( wsn_sched_mode == TASK_SCHED ) {
			wsn_LeaveCritical();
			Wsn_TaskSched();
		}

#ifdef _Debug_thread_

		else if ( wsn_sched_mode == THREAD_SCHED ) {	  		
			if ( wsn_ready_tasks != 0 ) { // Switch to thread 0 
				wsn_sched_mode = TASK_SCHED;
				wsn_context_switch(&wsn_current_thread->context, &wsn_thread0_context);
			}
			
		}

#endif
		else if(wsn_sched_mode == BACKGROUND_SCHED){

#ifdef _Debug_thread_
			for(;;)
#endif
			{
	
				if(wsn_ready_tasks != 0){
					wsn_sched_mode = TASK_SCHED;
					wsn_LeaveCritical();
					Wsn_TaskSched();
				}
#ifdef _Debug_thread_
				if(wsn_current_thread != 0){
					wsn_sched_mode = THREAD_SCHED;
					wsn_context_switch(&wsn_thread0_context,&wsn_current_thread->context);
				}else if(wsn_thread_queue != 0){
					wsn_current_thread = wsn_thread_queue;
					wsn_thread_queue = wsn_current_thread->next;
				}

				if(wsn_ready_tasks == 0 && wsn_thread_queue ==0 && wsn_current_thread == 0)
					break;
#endif
			}
			wsn_sched_mode = BACKGROUND_SCHED;
		}
	}

}

void wsn_RequestResource(uint16 res_id,int8 taskid)
{
	/*debug code*/
	if(taskid != wsn_task_queue[0]->taskindex)
		while(1);
	wsn_EnterCritical();
	wsn_task_queue[0]->delta = wsn_resource_table[res_id - 1].delta;
	wsn_LeaveCritical();
	Wsn_TaskSched();
}

void wsn_ReleaseResource(int8 taskid)
{	
	//struct wsn_task_attr * p_attr;
	/*debug code*/
	if(taskid != wsn_task_queue[0]->taskindex)
		while(1);
		
	wsn_EnterCritical();
	//p_attr = TaskAttr(wsn_task_queue[0]->taskindex);
	//wsn_task_queue[0]->delta= flash_read_dword(p_attr->time_to_complete);
	wsn_task_queue[0]->delta= 0xffffffff;
	wsn_LeaveCritical();
	Wsn_TaskSched();
	
}



